#include "SphereCollider.h"
#include "BoxCollider.h"

namespace GeorgeLib
{
	SphereCollider::SphereCollider(float radius)
		: _radius(radius)
	{
	}

	bool SphereCollider::Collides(Collider* collider)
	{
		if (!enabled || this == collider) return false;

		SphereCollider* sphere = CastCollider(collider);
		BoxCollider* box = BoxCollider::CastCollider(collider);

		if (sphere != NULL)
		{
			// Sphere-Sphere collision
			if (_radius > 0 && sphere->_radius > 0)
				return (wPos - sphere->wPos).magnitudeSqr() <= _radiusSqr + sphere->_radiusSqr;
		}
		else if (box != NULL)
		{
			// Sphere-Box collision
			// Pseudo-code from http://www.mrtc.mdh.se/projects/3Dgraphics/paperF.pdf
			if (_radius > 0 && box->size() > vec3(0, 0, 0))
			{
				float dist = 0;
				vec3 boxMin = box->min(), boxMax = box->max();
				for (int i = 0; i < 3; ++i)
				{
					float e;
					if ((e = wPos[i] - boxMin[i]) < 0)
					{
						if (e < -_radius) return false;
						else dist += e * e;
					}
					else if ((e = wPos[i] - boxMax[i]) > 0)
					{
						if (e > _radius) return false;
						else dist += e * e;
					}
				}
				return dist <= _radiusSqr;
			}
		}
	}

	bool SphereCollider::Contains(Collider* collider)
	{
		if (!enabled || this == collider) return false;

		SphereCollider* sphere = CastCollider(collider);
		BoxCollider* box = BoxCollider::CastCollider(collider);

		if (sphere != NULL)
		{
			// Sphere-Sphere containment
			if (_radius > 0 && sphere->_radius > 0)
				return (wPos - sphere->wPos).magnitudeSqr() <= _radiusSqr - sphere->_radiusSqr;
		}
		else if (box != NULL)
		{
			// Sphere-Box containment
			if (_radius > 0 && box->size() > vec3(0, 0, 0))
			{
				// boxMin is more than the minimum coords that lie on the sphere
				// boxMax is less than the maximum coords that lie on the sphere
				return min().xyzLessThan(box->min()) && box->max().xyzLessThan(max());
			}
		}
	}

	bool SphereCollider::Contains(vec3 point)
	{
		if (!enabled) return false;

		return (point - wPos).magnitudeSqr() < _radiusSqr;
	}

	bool SphereCollider::IsSphere(const Collider& collider)
	{
		return dynamic_cast<const SphereCollider*>(&collider) != NULL;
	}

	SphereCollider* SphereCollider::CastCollider(Collider* const collider)
	{
		return dynamic_cast<SphereCollider*>(collider);
	}
}